//#include "printf.h"

module StrainSensorNewP {

	provides {
//		interface Read<uint16_t>;
		interface SerialSensorRead as Read;
		interface Msp430UartConfigure;
		interface Init ;
	}
	uses {



	  interface GeneralIO  as Power ;
	  interface UartStream as	SensorStream;
	  interface	Resource;


	  interface Timer<TMilli> as CommandInterleavingTimer ;
	  interface Timer<TMilli> as CommandTimeoutTimer ;

	  interface	Leds;

	}

}
implementation {

	msp430_uart_union_config_t msp430_uart_telos_config = {
		{ ubr: UBR_1MHZ_9600,
			umctl: UMCTL_1MHZ_9600,
			ssel: 0x02,
			pena: 0,
			pev: 0,
			spb: 0,
			clen: 1,
			listen: 0,
			mm: 0,
			ckpl: 0,
			urxse: 0,
			urxeie: 1,
			urxwie: 0,
			utxe : 1,
			urxe : 1}
	};


	enum{

		COMMAND_INTERLEAVING = 256U, //256,//2048, //2048,//512,
		COMMAND_TIMEOUT = 5120U,

		CMD_REQUEST_SAMPLE = 0x0,

		MAX_CHANNEL_RETRIES = 3,

		PARSER_INIT = 0,
		  PARSER_CHANNEL = 1 ,
		  PARSER_TIME_1 = 2 ,
		  PARSER_TIME_2 = 3 ,
		  PARSER_TIME_3 = 4 ,
		  PARSER_TIME_4 = 5 ,
		  PARSER_TIME_5 = 6 ,
		  PARSER_TEMP_1 = 7 ,
		  PARSER_TEMP_2 = 8 ,
		  PARSER_TEMP_3 = 9 ,
		  PARSER_TEMP_4 = 10 ,
		  PARSER_TEMP_5 = 11 ,

	};

	uint8_t sensor_channel[] = {1,2} ; // available channels

	serial_sensor_data_t sensor_data ;

	bool m_busy = FALSE;

	task void read();

	task void request_resource() ;
	task void request_sample() ;
	task void send_single_channel_request() ;

	static void parse(uint8_t) ;
	static bool isDigit(uint8_t) ;
	static bool isNewLine(uint8_t) ;
	static bool isDot(uint8_t) ;
	static void loadValues() ;


	task void sendCommand() ;

	task void switchChannel() ;


  uint8_t channel_index = 0 ;


  // These values are set to zero at each single channel request.

  uint8_t m_cmd ;

  uint32_t start ;
  uint32_t stop ;

  uint8_t cmd[3] ;

  uint16_t parser_temperature = 0 ;
  uint16_t parser_time = 0 ;
  uint16_t parser_channel = 0 ;


  uint8_t parser_state ;

  uint8_t channel_retries ;

  command error_t Init.init(){
	  call Power.makeOutput(); // this command should be placed at init or boot
	  return SUCCESS ;
  }

	command error_t Read.read() {
		if ( m_busy == TRUE ){
//			printf("%s, EBUSY\n", __FUNCTION__);
//			printfflush();
			return EBUSY;
		}

		start = call CommandInterleavingTimer.getNow() ;
		m_busy = TRUE;
		channel_index = 0 ;
		atomic{
			channel_retries = 0 ;
		}
		post read();

		return SUCCESS;
	}

task void request_resource(){
	call Resource.request();
}

task void readDone(){
//	uint8_t i ;
	call Leds.set( 0 );
	call Power.clr();
	call Resource.release() ; // never knows...

	stop = call CommandInterleavingTimer.getNow() ;

	atomic{
		sensor_data.sensor_timer = stop - start ;
	}

//	for(i = 0; i<SENSOR_COUNT; i++){
//		atomic{
//		printf("channel %u - freq: %u , temp: %u\n",sensor_channel[i],sensor_data.sensor[i].data, sensor_data.sensor[i].temp ) ;
//		}
//	}
//	printfflush() ;
	m_busy = FALSE ;
	signal Read.readDone(SUCCESS,&sensor_data) ;
}

	task void read(){

		uint8_t i ;

		for(i=0; i<6; i++){
atomic{
			sensor_data.sensor[i].data = 0xffff ;
			sensor_data.sensor[i].temp = 0xffff ;
}
		}

		// Power on the board
		call Leds.set( 7 );
		call Power.set();


		// By default it starts with the first channel
		post sendCommand() ;

	}

	event void CommandInterleavingTimer.fired(){
		switch(m_cmd){
			case CMD_REQUEST_SAMPLE:
				post request_sample() ;
			break;
		}
	}

	event void CommandTimeoutTimer.fired(){
		// This timer can fire only if we have sent a command  to the board and we received no reply
		// Possible problem: the command has not been sent or the board does not reply anymore
		// What should we do then? -> simply retry, there is the single channel timeout that will switch to another channel
//		printf("Command timeout ... release.\n") ;
//		printfflush() ;

		call Resource.release() ;
		atomic{
		if(++channel_retries >= MAX_CHANNEL_RETRIES){
			post switchChannel() ;
		}
		else{
			post sendCommand() ;
		}
		}
	}

	task void request_sample(){
		// reset values
		atomic{
			parser_state = PARSER_INIT ;
		}

//		printf("Request resource\n") ;
//		printfflush() ;

		// Ask the resource to send a command.
		call CommandTimeoutTimer.startOneShot(COMMAND_TIMEOUT) ; // we have 4 seconds to receive a reply
		if (call Resource.immediateRequest() != SUCCESS ) {
			post request_resource() ; // if it hangs here (i.e., the resource is never granted), the timeout releases the resource and send a new command.

			return ;
		}
		post send_single_channel_request() ;
	}

	task void switchChannel(){
		// Stop all active timers
		atomic{
			if(call CommandInterleavingTimer.isRunning()) call CommandInterleavingTimer.stop() ;
			if(call CommandTimeoutTimer.isRunning()) call CommandTimeoutTimer.stop() ;
		}

		// switch to next channel
		if(channel_index < 1){
			channel_index++ ;
//			printf("Next Channel: %u\n",sensor_channel[channel_index]) ;
//			printfflush() ;

			atomic{
				channel_retries = 0 ;
			}
			// starts the procedure for the next channel
			post sendCommand() ;
		}
		else{
			post readDone() ;
		}

	}


	task void send_single_channel_request(){
		cmd[0] = 'G';
		cmd[1] = sensor_channel[channel_index] + '0' ;
		cmd[2] = 13 ;
//		printf("Sending command\n") ;
//		printfflush() ;
		call SensorStream.send( cmd, 3);
	}

static void storeValues(){
	sensor_data.sensor[parser_channel-1].data = parser_time ;
	sensor_data.sensor[parser_channel-1].temp = parser_temperature ;
}


static bool isDigit(uint8_t value){
	if(value >= 48 && value <= 57) return TRUE ;
	else return FALSE ;
}

static bool isNewLine(uint8_t value){
	return (value == 10)? TRUE:FALSE ;
}

static bool isDot(uint8_t value){
	return (value == 46)? TRUE:FALSE ;
}

static uint8_t toInt(uint8_t value){
	return value - '0' ;
}

static void parse(uint8_t value){
	switch(parser_state){
	case PARSER_INIT:
		if(value >= 1 && value <= 2){
			parser_channel = value ;
			parser_state = PARSER_CHANNEL ;
		}
		// else keep PARSER_INIT state
	break;

	case PARSER_CHANNEL:
		if(isNewLine(value)){
			parser_state = PARSER_TIME_1 ;
		}
		else parser_state = PARSER_INIT ;
	break;

	case PARSER_TIME_1:
		if(isDigit(value)){
			parser_time = toInt(value) ;
			parser_state = PARSER_TIME_2 ;
		}
		else parser_state = PARSER_INIT ;
	break;

	case PARSER_TIME_2:
		if(isDigit(value)){
			parser_time = parser_time*10 + toInt(value) ;
			parser_state = PARSER_TIME_3 ;
		}
		else parser_state = PARSER_INIT ;
	break;

	case PARSER_TIME_3:
		if(isDigit(value)){
			parser_time = parser_time*10 + toInt(value) ;
			parser_state = PARSER_TIME_4 ;
		}
		else parser_state = PARSER_INIT ;
	break;

	case PARSER_TIME_4:
		if(isDigit(value)){
			parser_time = parser_time*10 + toInt(value) ;
			parser_state = PARSER_TIME_5 ;
		}
		else parser_state = PARSER_INIT ;
	break;

	case PARSER_TIME_5:
		if(isNewLine(value)){
			// check if the value is 0
			// If not, switch to temp, else reset parser.
			if(parser_time == 0){
				parser_state = PARSER_INIT ;
				call Resource.release() ;
				if(++channel_retries >= MAX_CHANNEL_RETRIES){
					post switchChannel() ;
				}
				else{
					post sendCommand() ;
				}
			}
			else parser_state = PARSER_TEMP_1 ;
		}
		else parser_state = PARSER_INIT ;
		break;

	case PARSER_TEMP_1:
		if(isDigit(value)){
			parser_temperature = toInt(value) ;
			parser_state = PARSER_TEMP_2 ;
		}
		else parser_state = PARSER_INIT ;
	break;

	case PARSER_TEMP_2:
		if(isDot(value)){
			parser_state = PARSER_TEMP_3 ;
		}
		else parser_state = PARSER_INIT ;
	break;

	case PARSER_TEMP_3:
		if(isDigit(value)){
			parser_temperature = parser_temperature*10 + toInt(value) ;
			parser_state = PARSER_TEMP_4 ;
		}
		else parser_state = PARSER_INIT ;
	break;

	case PARSER_TEMP_4:
		if(isDigit(value)){
			parser_temperature = parser_temperature*10 + toInt(value) ;
			parser_state = PARSER_TEMP_5 ;
		}
		else parser_state = PARSER_INIT ;
	break;

	case PARSER_TEMP_5:
		if(isNewLine(value)){
			// That's fine store values.
			storeValues() ;
			parser_state = PARSER_INIT ;

			call Resource.release() ;
			post switchChannel() ;
		}
		else parser_state = PARSER_INIT ;
	break;
	}
}

	async event void SensorStream.receivedByte( uint8_t data ) {

//		printf("_%u",data) ;
//		printfflush() ;

		parse(data) ;

	}

	task void sendCommand(){
		// Stop the (possibly) running timer since we are sending a new command.
		if(call CommandTimeoutTimer.isRunning()) call CommandTimeoutTimer.stop() ;
		m_cmd = CMD_REQUEST_SAMPLE ;
		call CommandInterleavingTimer.startOneShot(COMMAND_INTERLEAVING) ;
	}

	async event void SensorStream.sendDone( uint8_t* buf, uint16_t len, error_t error )
	{
		// There is the timeout running, if the sendDone returns FAIL, just leave the timeouttimer to expire.
	}

	async event void SensorStream.receiveDone( uint8_t* buf, uint16_t len, error_t error ) {}

	async command msp430_uart_union_config_t* Msp430UartConfigure.getConfig() {
		return & msp430_uart_telos_config;
	}

	event void Resource.granted() {
//		printf("Granted\n") ;
//		printfflush() ;
		// now we have the uart ready, we can send the command
		post send_single_channel_request() ;

	}

}
